package behavior;


import sensor.USensor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.robotics.subsumption.*;
import lejos.util.Delay;
import motor.barredora;
import motor.carrito;
//import lejos.util.Delay;

/**
 * @version   1.0

 * @author    Bruno Argenta, Esteban Arrua, Luis Galusso
 * @copyright (C) 2012 Bruno Argenta, Esteban Arrua, Luis Galusso
 * @contact   b.argenta@gmail.com, estarr2212@gmail.com, lgalusso@gmail.com
 *
 * @license        GNU/GPLv3
 * This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program.  If not, see http://www.gnu.org/licenses/.
 *
 *
*/


public class HitWall implements Behavior {

   
	private USensor usensor;
    private carrito carro;
    private CompassHTSensor compass;
    private float pos = 0;
    private float fin = 0;
    private barredora bar;
    
    public HitWall(USensor usensor, carrito carro,CompassHTSensor compass, barredora bar)
    {
       
    	this.usensor = usensor; 
    	this.carro = carro;     
    	this.compass= compass;
    	this.bar = bar;
   
    }

    public boolean takeControl() {   
    	//EL CONTROL SE TOMA CUANDO SE ESTA A UNA DISTANCIA MENOR QUE 10 CM DE LA PARED
    	//color.reconocer();
    	int val = usensor.getValor();    
    	//int ant = anterior;
    	//anterior= val;
        return (val < 10)/* || ((ant == 255)&&(val == 255))*/;
}

    public void suppress() {
    	    
    }

    public void action() {    	
    	
    	bar.desactivar();
    	pos = compass.getDegreesCartesian();
    	
    	carro.SetMovingSpeed(250);
    	carro.Backward();
    	Delay.msDelay(1500);    	
    	//carro.Stop();   
    	//CALCULO POSICION DESTINO
    	//double giro = Math.random();
    	/*if (((direccion == 0)&&(giro <= 0.6))||((direccion!=0)&&(giro<=0.4))){
    		direccion = 0;
    		fin = (pos + 60)%360;
    		carro.SpinLeft();
    		//SI LA POSICION INICIAL ES MAYOR A 270 (LA POSICION OBJETIVO VA A SER MENOR YA QUE LA SUMA ES MODULO 360)
        	//REPOSICIONO HASTA QUE LOS GRADOS SEAN MENORES QUE POSICION OBJETIVO
        	//ESTE CASO ES CUANDO OBTENGO EL ANGULO MODULO 360 HASTA QUE EN UN MOMENTO SE LLEGA A UN ANGULO MAYOR QUE CERO
        	//Y MENOR QUE LA POSICION FINAL (PASAMOS LOS 360 GRADOS)
        	if (pos > fin){    		
        		while (pos >= fin){    			
        			pos = compass.getDegreesCartesian();
        			.println(Float.toString(pos));    		
        		}
        	}    	
        	
        	//LA POSICION INICIAL MENOR QUE 270 (POR LO QUE EL REPOSICIONAMIENTO NO LLEGARA HASTA 360)
        	while (pos <= fin){   		
        		pos = compass.getDegreesCartesian();
        		.println(Float.toString(pos));    		
        	}
    	}else{
    		direccion = 1;*/
    		fin = pos - 60;
    		if (fin <=0){
    			fin += 360;
    		}
    		carro.SpinRight();
    		//SI LA POSICION INICIAL ES MAYOR A 270 (LA POSICION OBJETIVO VA A SER MENOR YA QUE LA SUMA ES MODULO 360)
        	//REPOSICIONO HASTA QUE LOS GRADOS SEAN MENORES QUE POSICION OBJETIVO
        	//ESTE CASO ES CUANDO OBTENGO EL ANGULO MODULO 360 HASTA QUE EN UN MOMENTO SE LLEGA A UN ANGULO MAYOR QUE CERO
        	//Y MENOR QUE LA POSICION FINAL (PASAMOS LOS 360 GRADOS)
        	if (pos < fin){    		
        		while (pos <= fin){    			
        			pos = compass.getDegreesCartesian();
        			//.println(Float.toString(pos));    		
        		}
        	}    	
        	
        	//LA POSICION INICIAL MENOR QUE 270 (POR LO QUE EL REPOSICIONAMIENTO NO LLEGARA HASTA 360)
        	while (pos >= fin){   		
        		pos = compass.getDegreesCartesian();
        		//.println(Float.toString(pos));    		
        	}
    		
    	//}
    	usensor.cleanDistances();
    	carro.setAgent(false);
    	bar.activar();
    	
     	//carro.Stop();
   }
}